#pragma once

#include "typedefs.h"
#include "vector.h"
#include "smatrix.h"
#include "matrix.h"

namespace g4
{
	template <typename T>
	class basic_vector;
	template <typename T>
	class basic_smatrix;
}

namespace g2 
{

template <typename T>
class basic_translation;
template <typename T>
class basic_rigid; //you need to #include <glib/g2/optimize.h> for matching in that class
template <typename T>
class basic_rigid_scale;
template <typename T>
class basic_affine; //you need to #include <glib/g2/smatrix.h> for matching in that class

//p2l_matcher finds the best transformation (inside given class) 
//for matching sets of points {Q} and set of lines {P,n}, each line is given by normal
//to the line (n) and one of its points (P):
//t(Q)*n = P*n
//Rach pair of points has associated weight also. The more the weight the more precise
//should the transformation match given pair.

// if you need to match sets {Q}, {P}, {n} many times and 
// only {P}-set is changing then for the second and later matches
// you can use faster functions clear_p() and match_p(...)

template <class C>
class p2l_matcher;

template <class C>
class p2l_matcher_base : public accumulator_typedefs<typename C::element_type>
{
public:
	typedef C transform;
};

template <typename T>
class p2l_matcher< basic_translation<T> > : public p2l_matcher_base< basic_translation<T> >
{
public:
	p2l_matcher();
	void clear();
  void clear_p();
  void match(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
  void match_p(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match_p(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
	bool get_transform(transform & tr) const;
private:
	smatrix nn;
	vector q, p;
};

template <typename T>
class p2l_matcher_rigid_base : public accumulator_typedefs<T>
{
public:
	p2l_matcher_rigid_base();
	void clear();
  void clear_p();
  void match(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
  void match_p(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match_p(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
protected:
	smatrix nn;
	vector p, l;
	matrix mm1;
	smatrix mm2;
};

template <typename T>
class p2l_matcher< basic_rigid<T> > : 
	public p2l_matcher_base< basic_rigid<T> >,
	public p2l_matcher_rigid_base<T>
{
public:
	bool get_transform(transform & tr) const;
};

template <typename T>
class p2l_matcher< basic_rigid_scale<T> > : 
	public p2l_matcher_base< basic_rigid_scale<T> >,
	public p2l_matcher_rigid_base<T>
{
public:
	bool get_transform(transform & tr) const;
};

template <typename T>
class p2l_matcher< basic_affine<T> > : public p2l_matcher_base< basic_affine<T> >
{
public:
	p2l_matcher();
	void clear();
  void clear_p();
  void match(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
  void match_p(const vector & Q, element Pn, const vector & n, element weight = 1);
	void match_p(const vector & Q, const vector & P, const vector & n, element weight = 1)
    { match(Q, scalar_product(P, n), n, weight); }
	bool get_transform(transform & tr) const;
private:
	typedef g4::basic_vector<element> vector4;
	typedef g4::basic_smatrix<element> smatrix4;
private:
	smatrix nn;
	vector p;
	vector4 l;
	vector mm1[4]; //matrix 2x4
	smatrix4 mm2;
};

///////////////////////////
// inline implementation //
///////////////////////////

//
// translation p2l_matcher
//

template <typename T>
inline p2l_matcher< basic_translation<T> >::p2l_matcher()
	{ clear(); }

template <typename T>
inline void p2l_matcher< basic_translation<T> >::clear()
{
	nn.zero();
	q.zero();
  clear_p();
}

template <typename T>
inline void p2l_matcher< basic_translation<T> >::clear_p()
{
	p.zero();
}

template <typename T>
inline void p2l_matcher< basic_translation<T> >::match(const vector & Q, element Pn, const vector & n, element weight)
{
	nn += weight * tsquare(n);
  q += weight * scalar_product(Q, n) * n;
  match_p(Q, Pn, n, weight);
}

template <typename T>
inline void p2l_matcher< basic_translation<T> >::match_p(const vector & Q, element Pn, const vector & n, element weight)
{
  p += weight * Pn * n;
}

template <typename T>
inline bool p2l_matcher< basic_translation<T> >::get_transform(transform & tr) const
{
	element det = nn.det();
	if (det <= 0)
		return false;

	tr = transform(nn.inverse(det) * (p - q));
	return true;
}


//
// rigid base
//

template <typename T>
inline p2l_matcher_rigid_base<T>::p2l_matcher_rigid_base()
	{ clear(); }

template <typename T>
inline void p2l_matcher_rigid_base<T>::clear()
{
	nn.zero();
	mm1.zero();
	mm2.zero();
  clear_p();
}

template <typename T>
inline void p2l_matcher_rigid_base<T>::clear_p()
{
	p.zero();
	l.zero();
}

template <typename T>
inline void p2l_matcher_rigid_base<T>::match(const vector & Q, element Pn, const vector & n, element weight)
{
	nn += weight * tsquare(n);
	element wpn = weight * Pn;
	p += wpn * n;
	vector q(scalar_product(Q, n), outer_product(Q, n));
	mm1 += tproduct(weight * n, q);
	mm2 += weight * tsquare(q);
	l += wpn * q;
}

template <typename T>
inline void p2l_matcher_rigid_base<T>::match_p(const vector & Q, element Pn, const vector & n, element weight)
{
	element wpn = weight * Pn;
	p += wpn * n;
	vector q(scalar_product(Q, n), outer_product(Q, n));
	l += wpn * q;
}

//
// rigid p2l_matcher
//

template <typename T>
inline bool p2l_matcher< basic_rigid<T> >::get_transform(transform & tr) const
{
	element det_nn = nn.det();
	if (det_nn <= 0)
		return false;
	smatrix inv_nn = nn.inverse(det_nn);

	smatrix a = mm2 - duplex_product(inv_nn, mm1);
	element det_a = a.det();
	if (det_a <= 0)
		return false;

	vector b = l - (inv_nn * p) * mm1;
	vector x;
	element phi = optimal_angle(a, -b, &x); //#include <glib/g2/optimize.h>

	tr = transform(basic_rotation<T>(phi, x.x, x.y), inv_nn * (p - mm1 * x));
	return true;
}


//
// rigid+scale p2l_matcher
//

template <typename T>
inline bool p2l_matcher< basic_rigid_scale<T> >::get_transform(transform & tr) const
{
	element det_nn = nn.det();
	if (det_nn <= 0)
		return false;
	smatrix inv_nn = nn.inverse(det_nn);

	smatrix a = mm2 - duplex_product(inv_nn, mm1);
	element det_a = a.det();
	if (det_a <= 0)
		return false;

	vector b = l - (inv_nn * p) * mm1;
	vector x = a.inverse(det_a) * b;

	tr = transform(x.x, x.y, inv_nn * (p - mm1 * x));
	return true;
}


//
// affine p2l_matcher
//

template <typename T>
inline p2l_matcher< basic_affine<T> >::p2l_matcher()
	{ clear(); }

template <typename T>
inline void p2l_matcher< basic_affine<T> >::clear()
{
	nn.zero();
	for (int i = 0; i < 4; ++i)
		mm1[i].zero();
	mm2.zero();
  clear_p();
}

template <typename T>
inline void p2l_matcher< basic_affine<T> >::clear_p()
{
	p.zero();
	l.zero();
}

template <typename T>
inline void p2l_matcher< basic_affine<T> >::match(const vector & Q, element Pn, const vector & n, element weight)
{
	nn += weight * tsquare(n);
	vector wn = weight * n;
	element wpn = weight * Pn;
	p += wpn * n;
	vector4 q(Q.x * n.x, Q.y * n.x, Q.x * n.y, Q.y * n.y);
	mm1[0] += wn * q.x;
	mm1[1] += wn * q.y;
	mm1[2] += wn * q.z;
	mm1[3] += wn * q.t;
  mm2 += weight * g4::tsquare(q);
	l += wpn * q;
}

template <typename T>
inline void p2l_matcher< basic_affine<T> >::match_p(const vector & Q, element Pn, const vector & n, element weight)
{
	element wpn = weight * Pn;
	p += wpn * n;
	vector4 q(Q.x * n.x, Q.y * n.x, Q.x * n.y, Q.y * n.y);
	l += wpn * q;
}

template <typename T>
inline bool p2l_matcher< basic_affine<T> >::get_transform(transform & tr) const
{
	element det_nn = nn.det();
	if (det_nn <= 0)
		return false;
	smatrix inv_nn = nn.inverse(det_nn);

	// inv_nn * mm1
	vector nm[4];
	for (int i = 0; i < 4; ++i)
		nm[i] = inv_nn * mm1[i];

	// a = mm2 - duplex_product(inv_nn, mm1);
	smatrix4 a = mm2 - smatrix4(
		scalar_product(mm1[0], nm[0]), //xx
		scalar_product(mm1[0], nm[1]), //xy
		scalar_product(mm1[0], nm[2]), //xz
		scalar_product(mm1[0], nm[3]), //xt
		scalar_product(mm1[1], nm[1]), //yy
		scalar_product(mm1[1], nm[2]), //yz
		scalar_product(mm1[1], nm[3]), //yt
		scalar_product(mm1[2], nm[2]), //zz
		scalar_product(mm1[2], nm[3]), //zt
		scalar_product(mm1[3], nm[3])  //tt
	);

	element det_a = a.det();
	if (det_a <= 0)
		return false;

	vector np = inv_nn * p;
	// b = l - (inv_nn * p) * mm1;
	vector4 b = l - vector4(
		scalar_product(mm1[0], np),
		scalar_product(mm1[1], np),
		scalar_product(mm1[2], np),
		scalar_product(mm1[3], np)
	);
	vector4 x = a.inverse(det_a) * b;
	// mm1 * x
	vector mx(
		mm1[0].x * x.x + mm1[1].x * x.y + mm1[2].x * x.z + mm1[3].x * x.t,
		mm1[0].y * x.x + mm1[1].y * x.y + mm1[2].y * x.z + mm1[3].y * x.t);

	tr = transform(matrix(x.x, x.y, x.z, x.t), inv_nn * (p - mx));
	return true;
}

} //namespace g2
